/*倒立摆控制与仿真*/
#include "pendulum.hpp"
#include "controller.hpp"
#include "graphics.h"

#define SCENE_HALF_WIDTH  512  // 画面宽度/2
#define SCENE_HALF_HEIGHT  120  // 画面高度/2
#define SCENE_WIDTH  (SCENE_HALF_WIDTH*2)  // 画面宽度
#define SCENE_HEIGHT  (SCENE_HALF_HEIGHT*2)  // 画面高度
#define CAR_HALF_WIDTH  20  // 小车宽度/2
#define CAR_HALF_HEIGHT  10  // 小车高度/2
#define CAR_ALTITUDE  10  // 小车底部与画面底部的距离
#define ROD_LENGTH  180  // 摆杆长度
#define CAR_INITIAL_X  200  // 小车起始位置(相对于中点)


struct PendulumState{
    PendulumState() {};
    PendulumState(double x, double theta): x(x), theta(theta) {}
    double x, theta;
};

class Pendulum_Simulator
{
public:
    Pendulum_Simulator() {
        mpidinner = new PID_Controller(&sim1, 10, 0, 100, 100);
        mpidouter = new PID_Controller(&sim1, 0.64056276, 2.6843531, 0.80895114, 0.75105566);
        mpendulum = new InversePendulum(&sim1);
        SUConstant(minu, &sim1);
        SUSum(msuminner, &sim1);
        SUSum(msumouter, &sim1);
        SUOutput(mouttheta, &sim1);
        SUOutput(moutx, &sim1);
        minu->Set_OutValue(0);
        sim1.connectU(minu, msumouter);
        sim1.connectU(msumouter, mpidouter, 0);
        sim1.connectU(mpidouter, 0, msuminner);
        sim1.connectU(msuminner, mpidinner, 0);
        sim1.connectU(mpidinner, 0, mpendulum, 0);
        sim1.connectU(mpendulum, 0, msumouter);
        sim1.connectU(mpendulum, 2, msuminner);
        sim1.connectU(minu, mpendulum, 0);
        msumouter->Set_InputGain(-1);
        msuminner->Set_InputGain(-1);
        sim1.connectU(mpendulum, 0, moutx);
        sim1.connectU(mpendulum, 2, mouttheta);
        sim1.Set_EnableStore(false);
        sim1.Initialize();
        _simstep = sim1.Get_SimStep();
    };
    PendulumState Simulate(double time) {
        int stepcnt = (int)(time/_simstep/1000.0+0.5);
        for (int i=0; i<stepcnt; ++i)
            sim1.Simulate_OneStep();
        return PendulumState(moutx->Get_OutValue(), mouttheta->Get_OutValue());
    }
    void Reset(void){
        sim1.Simulation_Reset();
    }
private:
    Simulator sim1;
    PID_Controller *mpidinner=nullptr;  // 内环PID控制器
    PID_Controller *mpidouter=nullptr;  // 外环PID控制器
    InversePendulum *mpendulum=nullptr;  // 倒立摆系统
    // MInput *minu=nullptr;  // 输入期望角度
    UConstant *minu=nullptr;  // 输入期望角度
    USum *msuminner=nullptr;  // 角度误差
    USum *msumouter=nullptr;  // 位置误差
    UOutput *mouttheta=nullptr;  // 摆杆角度
    UOutput *moutx=nullptr;  // 小车位移
    double _simstep;
};

int Draw_Pendulum(PendulumState car)
{
    // car.x = 0;
    int cx = int(car.x*100+0.5) + SCENE_HALF_WIDTH + CAR_INITIAL_X;
    int cy = SCENE_HEIGHT - CAR_ALTITUDE - CAR_HALF_HEIGHT;
    bar(cx-CAR_HALF_WIDTH, cy-CAR_HALF_HEIGHT, cx+CAR_HALF_WIDTH, cy+CAR_HALF_HEIGHT);
    cy -= CAR_HALF_HEIGHT;
    int bx = cx - int(ROD_LENGTH * sin(car.theta) + 0.5);
    int by = cy - int(ROD_LENGTH * cos(car.theta) + 0.5);
    line(bx, by, cx, cy);
    if ((cx<-CAR_HALF_WIDTH) || (cx>SCENE_WIDTH+CAR_HALF_WIDTH)) return 1;
    return 0;
}

int main()
{
    Pendulum_Simulator psim1;
    PendulumState car1(0, 0);
    double msdelays = 5;
    double totaltime = 0;
    initgraph(SCENE_WIDTH, SCENE_HEIGHT);
    setbkcolor(0xFFFFFF);
    setcolor(0);
    setfillcolor(0);
    for (; is_run(); delay_ms(msdelays)) {
        car1 = psim1.Simulate(msdelays);
        cleardevice();
        if (Draw_Pendulum(car1))
            psim1.Reset();
        else if (totaltime>5000) {
            psim1.Reset();
            totaltime = 0;
        }
        totaltime += msdelays;
    }
    return 0;
}
